#include "T1000.h"
#include "pal/palFactory.h"
#include "pal/ConfigStatic.h"
#include "elementconfig.h"

namespace robots
{

T1000::T1000()
{
}

T1000::~T1000()
{
	delete[] cGeometry;
	delete[] gGeometry;
}

int T1000::getElementCount()
{
	return 5;
}

ElementData T1000::getElementData(int index)
{
	palMatrix4x4 loc = cGeometry[index]->GetLocationMatrix();
	gGeometry[index]->setTransformationMatrix(loc._mat);
	return (*gGeometry[index]);
}

void T1000::move(float x, float y, float z)
{
}

void T1000::command(int commandID, int n, void* args)
{
	cGeometry[0]->SetActive(true);
	float* v;
	if (n == 1) {
		v = (float*) args;
	} else {
		v = new float[1];
		v[0] = 100;
	}
	switch (commandID)
	{
	case 1:
		pa1->Update(v[0]);
		pa2->Update(v[0]);
		pa1->Apply();
		pa2->Apply();
		break;
	case 2:
		pa1->Update(-v[0]);
		pa2->Update(-v[0]);
		pa1->Apply();
		pa2->Apply();

		break;
	case 3:
		pa1->Update(-v[0]);
		pa2->Update(v[0]);
		pa1->Apply();
		pa2->Apply();

		break;
	case 4:
		pa1->Update(v[0]);
		pa2->Update(-v[0]);
		pa1->Apply();
		pa2->Apply();

		break;
	case 0:
		pa1->Update(0);
		pa2->Update(0);
		pa1->Apply();
		pa2->Apply();

		break;
	default:

		break;
	}
}

void T1000::setPosition(float x, float y, float z)
{
	position.x = x;
	position.y = y;
	position.z = z;
}

palVector3 T1000::getPosition()
{
	palVector3 pos;
	pos.x = position.x;
	pos.y = position.y;
	pos.z = position.z;
	return pos;
}

void T1000::createPhysics()
{
	palBox *mainBody = PF->CreateBox();
	mainBody->Init(position.x, position.y, position.z, 1.5, 0.5, 1.7, 40);
	cGeometry[0] = mainBody;
	ElementData *bodyElement = new ElementData(BoxType);
	float *edata = new float[3];
	edata[0] = 1.5;
	edata[1] = 0.5;
	edata[2] = 1.7;
	bodyElement->setData(3,edata);
	delete [] edata;
	palMatrix4x4 loc = mainBody->GetLocationMatrix();
	bodyElement->setTransformationMatrix(loc._mat);
	bodyElement->setColor(0,200,200);
	gGeometry[0] = bodyElement;

	palSphere *wheel1 = PF->CreateSphere();
	wheel1->Init(position.x + 1.5, position.y - 0.1, position.z + 0.5, 0.5, 5);
	cGeometry[1] = wheel1;
	ElementData *weelElement = new ElementData(SphereType);
	edata = new float[1];
	edata[0] = 0.5;
	weelElement->setData(1, edata);
	weelElement->setColor(0,0,0);
	delete[] edata;
	loc = wheel1->GetLocationMatrix();
	weelElement->setTransformationMatrix(loc._mat);
	gGeometry[1] = weelElement;

	palSphere *wheel2 = PF->CreateSphere();
	wheel2->Init(position.x + 1.5, position.y - 0.1, position.z - 1, 0.5, 5);
	cGeometry[2] = wheel2;
	weelElement = new ElementData(SphereType);
	edata = new float[1];
	edata[0] = 0.5;
	weelElement->setData(1, edata);
	weelElement->setColor(0,0,0);
	delete[] edata;
	loc = wheel2->GetLocationMatrix();
	weelElement->setTransformationMatrix(loc._mat);
	gGeometry[2] = weelElement;

	palSphere *wheel3 = PF->CreateSphere();
	wheel3->Init(position.x - 1.5, position.y - 0.1, position.z + 0.5, 0.5, 5);
	cGeometry[3] = wheel3;
	weelElement = new ElementData(SphereType);
	edata = new float[1];
	edata[0] = 0.5;
	weelElement->setData(1, edata);
	weelElement->setColor(0,0,0);
	delete[] edata;
	loc = wheel3->GetLocationMatrix();
	weelElement->setTransformationMatrix(loc._mat);
	gGeometry[3] = weelElement;

	palSphere *wheel4 = PF->CreateSphere();
	wheel4->Init(position.x - 1.5, position.y - 0.1, position.z - 1, 0.5, 5);
	cGeometry[4] = wheel4;
	weelElement = new ElementData(SphereType);
	edata = new float[1];
	edata[0] = 0.5;
	weelElement->setData(1, edata);
	weelElement->setColor(0,0,0);
	delete[] edata;
	loc = wheel4->GetLocationMatrix();
	weelElement->setTransformationMatrix(loc._mat);
	gGeometry[4] = weelElement;

	palRevoluteLink *prl0;
	prl0
			= dynamic_cast<palRevoluteLink *> (PF->CreateObject(
					"palRevoluteLink"));
	prl0->Init(mainBody, wheel1, position.x + 1.5, position.y - 0.1, position.z
			+ 0.5, 1, 0, 0);

	palSphericalLink *prl1;
	prl1 = dynamic_cast<palSphericalLink *> (PF->CreateObject(
			"palSphericalLink"));
	prl1->Init(mainBody, wheel2, position.x + 1.5, position.y - 0.1, position.z
			- 1);

	palRevoluteLink *prl2;
	prl2
			= dynamic_cast<palRevoluteLink *> (PF->CreateObject(
					"palRevoluteLink"));
	prl2->Init(mainBody, wheel3, position.x - 1.5, position.y - 0.1, position.z
			+ 0.5, 1, 0, 0);

	palSphericalLink *prl3;
	prl3 = dynamic_cast<palSphericalLink *> (PF->CreateObject(
			"palSphericalLink"));
	prl3->Init(mainBody, wheel4, position.x - 1.5, position.y - 0.1, position.z
			- 1);

	pa1 = dynamic_cast<palAngularMotor *> (PF->CreateObject("palAngularMotor"));
	pa1->Init(prl2, 50.5f);
	pa2 = dynamic_cast<palAngularMotor *> (PF->CreateObject("palAngularMotor"));
	pa2->Init(prl0, 50.5f);
	//	for (int i = 0)

}

}
